Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(start_planner): keep distance back static objects from start pose #6149

Open
wants to merge 14 commits into
base: main
Choose a base branch
from

Conversation

kyoichi-sugahara
Copy link
Contributor

@kyoichi-sugahara kyoichi-sugahara commented Jan 23, 2024

Description

Following launcher PR should be merged first
autowarefoundation/autoware_launch#815

keep enough distance between back static object and start pose.
This distance is necessary for avoiding stack which is caused by other module's stop point insertion.

NOTE: margin against front objects can be smaller than margin against back objects, because start_planner expect the other module like avoidance to change the trajectory.

image

Tests performed

Effects on system behavior

Not applicable.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

  • There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

@github-actions github-actions bot added type:documentation Creating or refining documentation. (auto-assigned) component:planning Route planning, decision-making, and navigation. (auto-assigned) labels Jan 23, 2024
@kyoichi-sugahara kyoichi-sugahara added the tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Feb 7, 2024
Copy link

codecov bot commented Feb 7, 2024

Codecov Report

Attention: Patch coverage is 0% with 6 lines in your changes are missing coverage. Please review.

Project coverage is 14.86%. Comparing base (9530c8a) to head (e27daf8).
Report is 282 commits behind head on main.

❗ Current head e27daf8 differs from pull request most recent head 29571ff. Consider uploading reports for the commit 29571ff to get more accurate results

Files Patch % Lines
...behavior_path_start_planner_module/src/manager.cpp 0.00% 3 Missing ⚠️
..._start_planner_module/src/start_planner_module.cpp 0.00% 3 Missing ⚠️
Additional details and impacted files
@@            Coverage Diff             @@
##             main    #6149      +/-   ##
==========================================
- Coverage   14.99%   14.86%   -0.13%     
==========================================
  Files        1839     1845       +6     
  Lines      127213   126613     -600     
  Branches    38132    37871     -261     
==========================================
- Hits        19071    18822     -249     
+ Misses      86808    86634     -174     
+ Partials    21334    21157     -177     
Flag Coverage Δ *Carryforward flag
differential 0.00% <0.00%> (?)
total 14.86% <ø> (-0.13%) ⬇️ Carriedforward from e2bd68f

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

@kyoichi-sugahara
Copy link
Contributor Author

@kosuke55
confirmed no degression with evaluator's result.
Could you review the PR?

@kyoichi-sugahara kyoichi-sugahara removed the tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Feb 8, 2024
Signed-off-by: kyoichi-sugahara <[email protected]>
@kyoichi-sugahara kyoichi-sugahara force-pushed the feat/keep_distance_back_static_objects_from_start_pose branch from 3d2df33 to 098b6fd Compare February 8, 2024 18:27
@kyoichi-sugahara kyoichi-sugahara marked this pull request as draft February 9, 2024 16:53
@kyoichi-sugahara kyoichi-sugahara marked this pull request as ready for review February 9, 2024 19:52
Comment on lines +115 to +125
for (const auto & obj : static_objects.objects) {
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj);
for (const auto & obj_outer_point : obj_polygon.outer()) {
const auto obj_pose_arc_length = getArcLengthForPoint(lanelets, obj_outer_point);
for (const auto & vehicle_corner_point : vehicle_footprint) {
const auto vehicle_pose_arc_length = getArcLengthForPoint(lanelets, vehicle_corner_point);
const double distance = std::abs(obj_pose_arc_length - vehicle_pose_arc_length);
min_distance = std::min(min_distance, distance);
}
}
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nits:
this could be heavy.
I think there are better ways to make it faster, such as extracting the closest object, the closest point, etc.

Copy link
Contributor

@kosuke55 kosuke55 Feb 11, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

pseudo code

backward_objects = extraceBackwardObjects(lanelets, ego_pose, objectse); // extract objects whose arc length is less than ego's one
closest_backward_object = getClosestObject(backward_objects); // get object whose arc length is max
closet_object_corner_point = getClosestCornerPoint(closest_backward_object); // get cornaner whose arc length is max
most_backward_ego_corner_point = getMostBackwardEgoCornerPoint(ego_pose, vehicle_footprint); // get corner whose arc length is min
distance = most_backward_ego_corner_point - closet_object_corner_point; // get arc length between two points

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks!
I will consider improving the efficiency

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In my current implementation, distance a is calculated but distance b should be calculated, correct?
If so, the method of extracting objects for back distance against back object should be changed. In this sketch, only object [2] should be extracted.
image

And for caring lateral collision implemented with the following implementetion. objects[0,1,2] should be extracted 🤔

      (utils::checkCollisionBetweenFootprintAndObjects(
        local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes,
        parameters_->objects_collision_check_margins.back())))

Copy link
Contributor

@kosuke55 kosuke55 Feb 11, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I will consider improving the efficiency

thanks! but it's ok if it takes time to improve because the check target objects may be not so many

In my current implementation, distance a is calculated but distance b should be calculated, correct?

no, I think distance a should be calculated,

And for caring lateral collision implemented with the following implementetion. objects[0,1,2] should be extracted 🤔

I agree

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

how about creating polygon along the start_pose_candidates_path?

const PathWithLaneId start_pose_candidates_path = calcBackwardPathFromStartPose();

And extracts closest object overlaps with the polygon
image
a: objects_collision_check_margins.back()
b: back_objects_collision_check_margin

It's heavier but checking lateral distance of object pose is not considering the size of the object?

Copy link
Contributor

@kosuke55 kosuke55 Feb 12, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

looks good!
the attention area is along the start_pose_candidates_path not just nagative x-coords direction, right?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

which pattern did you mean?
image

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

and, better to check just side to ego?

image

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(sorry for my mistake, this pseudo code did not consider footprint)

#6149 (comment)

    const auto obj_arc_coods = getArcCoordinates(lanelets, object.pose.position);
    if(std::abs(ego_arc_coords.distance -obj_arc_coods.distance) > lateral_threshold){
      continue;
    }

not just check object.pose.position but need to check 4 footprint points by like std::any

Copy link

stale bot commented Apr 17, 2024

This pull request has been automatically marked as stale because it has not had recent activity.

@stale stale bot added the status:stale Inactive or outdated issues. (auto-assigned) label Apr 17, 2024
@xmfcx
Copy link
Contributor

xmfcx commented Dec 10, 2024

@kyoichi-sugahara @kosuke55 could you rebase and work on this PR?

@stale stale bot removed the status:stale Inactive or outdated issues. (auto-assigned) label Dec 10, 2024
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component:planning Route planning, decision-making, and navigation. (auto-assigned) type:documentation Creating or refining documentation. (auto-assigned)
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants